#include "CirclesAxisCalbration.h"

#include <iostream>

#include <opencv2/core.hpp>
#include <Eigen/Dense>

using namespace std;
using namespace cv;

bool CirclesAxisCalbration::calibCirclePlanesNorm(std::vector<Eigen::Vector3d> &pAs, std::vector<Eigen::Vector3d> &pBs, Eigen::Vector3d &outNorm)
{
    uint nPoint = min(pAs.size(), pBs.size());
    if (nPoint < 3) {
        return false;
    }

    Eigen::Vector3d pMeanA{0, 0, 0},  pMeanB{0, 0, 0};
    for(uint i=0; i< nPoint; i++) {
        pMeanA += pAs[i];
        pMeanB += pBs[i];
    }
    pMeanA /= nPoint;
    pMeanB /= nPoint;

    Mat AA = Mat((nPoint)*2,3,CV_64F);
    for(uint i=0; i < nPoint; i++) {
        Eigen::Vector3d pA = pAs[i] - pMeanA;
        Eigen::Vector3d pB = pBs[i] - pMeanB;

        double *pAA = AA.ptr<double>(i*2);
        pAA[0] = pA[0];  pAA[1] =pA[1];  pAA[2] = pA[2];
        pAA = AA.ptr<double>(i*2+1);
        pAA[0] = pB[0];  pAA[1] = pB[1];  pAA[2] = pB[2];
    }
    Mat XX = Mat(3,1,CV_64F);
    SVD::solveZ(AA, XX);
    std::cout << XX << endl;
    double *pXX = XX.ptr<double>();
    outNorm[0] = pXX[0];
    outNorm[1] = pXX[1];
    outNorm[2] = pXX[2];
    return true;
}

bool CirclesAxisCalbration::calibCirclePlanesNorm2(std::vector<Eigen::Vector3d> &pAs, std::vector<Eigen::Vector3d> &pBs, Eigen::Vector3d &outNorm)
{
    uint nPointA = pAs.size();
    uint nPointB =  pBs.size();
    if (nPointA < 3 || nPointB < 3) {
        return false;
    }

    Mat AA = Mat(8,5,CV_64F);
    double *pAA;
    {
        double sumX = 0, sumY = 0, sumZ = 0;
        double sumXX = 0, sumXY = 0, sumXZ = 0;
        double            sumYY = 0, sumYZ = 0;
        double                       sumZZ = 0;
        for(uint i=0; i< nPointA; i++) {
            Eigen::Vector3d p = pAs[i];
            sumX += p[0]; sumY += p[1]; sumZ += p[2];
            sumXX += p[0]*p[0]; sumXY += p[0]*p[1]; sumXZ += p[0]*p[2];
            sumYY += p[1]*p[1]; sumYZ += p[1]*p[2];
            sumZZ += p[2]*p[2];
        }

        pAA = AA.ptr<double>(0);
        pAA[0] = sumXX; pAA[1] = sumXY; pAA[2] = sumXZ; pAA[3] = sumX; pAA[4] = 0;
        pAA = AA.ptr<double>(1);
        pAA[0] = sumXY; pAA[1] = sumYY; pAA[2] = sumYZ; pAA[3] = sumY; pAA[4] = 0;
        pAA = AA.ptr<double>(2);
        pAA[0] = sumXZ; pAA[1] = sumYZ; pAA[2] = sumZZ; pAA[3] = sumZ; pAA[4] = 0;
        pAA = AA.ptr<double>(3);
        pAA[0] = sumX; pAA[1] = sumY; pAA[2] = sumZ; pAA[3] = nPointA;  pAA[4] = 0;
    }

    {
        double sumX = 0, sumY = 0, sumZ = 0;
        double sumXX = 0, sumXY = 0, sumXZ = 0;
        double            sumYY = 0, sumYZ = 0;
        double                       sumZZ = 0;
        for(uint i=0; i< nPointB; i++) {
            Eigen::Vector3d p = pBs[i];
            sumX += p[0]; sumY += p[1]; sumZ += p[2];
            sumXX += p[0]*p[0]; sumXY += p[0]*p[1]; sumXZ += p[0]*p[2];
            sumYY += p[1]*p[1]; sumYZ += p[1]*p[2];
            sumZZ += p[2]*p[2];
        }

        pAA = AA.ptr<double>(4);
        pAA[0] = sumXX; pAA[1] = sumXY; pAA[2] = sumXZ; pAA[4] = sumX; pAA[3] = 0;
        pAA = AA.ptr<double>(5);
        pAA[0] = sumXY; pAA[1] = sumYY; pAA[2] = sumYZ; pAA[4] = sumY; pAA[3] = 0;
        pAA = AA.ptr<double>(6);
        pAA[0] = sumXZ; pAA[1] = sumYZ; pAA[2] = sumZZ; pAA[4] = sumZ; pAA[3] = 0;
        pAA = AA.ptr<double>(7);
        pAA[0] = sumX; pAA[1] = sumY; pAA[2] = sumZ; pAA[4] = nPointB;  pAA[3] = 0;
    }

    Mat XX = Mat(5,1,CV_64F);
    SVD::solveZ(AA, XX);
    std::cout << XX << endl;
    double *pXX = XX.ptr<double>();
    outNorm[0] = pXX[0];
    outNorm[1] = pXX[1];
    outNorm[2] = pXX[2];
    return true;
}

bool CirclesAxisCalbration::calibPlanesNorm(std::vector<Eigen::Vector3d> &pAs, Eigen::Vector3d &outNorm)
{
    uint nPoint = pAs.size();
    if (nPoint < 3) {
        return false;
    }
    double sumX = 0, sumY = 0, sumZ = 0;
    double sumXX = 0, sumXY = 0, sumXZ = 0;
    double            sumYY = 0, sumYZ = 0;
    double                       sumZZ = 0;
    for(uint i=0; i< nPoint; i++) {
        Eigen::Vector3d p = pAs[i];
        sumX += p[0]; sumY += p[1]; sumZ += p[2];
        sumXX += p[0]*p[0]; sumXY += p[0]*p[1]; sumXZ += p[0]*p[2];
        sumYY += p[1]*p[1]; sumYZ += p[1]*p[2];
        sumZZ += p[2]*p[2];
    }

    Mat AA = Mat(4,4,CV_64F);
    double *pAA = AA.ptr<double>(0);
    pAA[0] = sumXX; pAA[1] = sumXY; pAA[2] = sumXZ; pAA[3] = sumX;
    pAA = AA.ptr<double>(1);
    pAA[0] = sumXY; pAA[1] = sumYY; pAA[2] = sumYZ; pAA[3] = sumY;
    pAA = AA.ptr<double>(2);
    pAA[0] = sumXZ; pAA[1] = sumYZ; pAA[2] = sumZZ; pAA[3] = sumZ;
    pAA = AA.ptr<double>(3);
    pAA[0] = sumX; pAA[1] = sumY; pAA[2] = sumZ; pAA[3] = nPoint;

    Mat XX = Mat(4,1,CV_64F);
    SVD::solveZ(AA, XX);
    std::cout << XX << endl;
    double *pXX = XX.ptr<double>();
    outNorm[0] = pXX[0];
    outNorm[1] = pXX[1];
    outNorm[2] = pXX[2];
    return true;
}

bool CirclesAxisCalbration::calibCirclesAxis(std::vector<Eigen::Vector3d> &pAs, std::vector<Eigen::Vector3d> &pBs,
                 Eigen::Vector3d &outCenterA, Eigen::Vector3d &outCenterB)
{
    uint nPointA = pAs.size();
    uint nPointB =  pBs.size();
    if (nPointA < 3 || nPointB < 3) {
        return false;
    }

    Eigen::Vector3d normV;
    if (!calibCirclePlanesNorm2(pAs, pBs, normV)) {
        return false;
    }

    if (!calibCircleCenter(pAs, normV, outCenterA)) {
        return false;
    }
    return calibCircleCenter(pBs, normV, outCenterB);
}

bool CirclesAxisCalbration::calibCircleCenter(std::vector<Eigen::Vector3d> &pAs, Eigen::Vector3d &outCenterA)
{
    Eigen::Vector3d n;
    if (!calibPlanesNorm(pAs, n)) {
        return false;
    }

    return calibCircleCenter(pAs, n, outCenterA);
}

bool CirclesAxisCalbration::calibCircleCenter(std::vector<Eigen::Vector3d> &pAs, const Eigen::Vector3d &n, Eigen::Vector3d &outCenterA)
{
    uint nPoint = pAs.size();
    if (nPoint < 3) {
        return false;
    }
    Eigen::Vector3d p0 = pAs[0];

    Mat AA = Mat((nPoint-1)*2,3,CV_64F);
    Mat BB = Mat((nPoint-1)*2,1,CV_64F);
    for(uint i=1; i < nPoint; i++) {
        p0 = pAs[i-1];
        double *pAA = AA.ptr<double>((i-1)*2);
        pAA[0] = n[0];  pAA[1] = n[1];  pAA[2] = n[2];

        pAA = AA.ptr<double>((i-1)*2+1);

        Eigen::Vector3d pi1 = pAs[i] - p0;
        pAA[0] = 2*pi1[0];
        pAA[1] = 2*pi1[1];
        pAA[2] = 2*pi1[2];
        BB.at<double>((i-1)*2,0) = pAs[i].dot(n);
        BB.at<double>((i-1)*2+1,0) = pAs[i].dot(pAs[i]) - p0.dot(p0);
    }
    Mat XX = Mat(3,1,CV_64F);

    solve(AA, BB, XX, DECOMP_SVD);
    std::cout << XX << endl;
    double *pXX = XX.ptr<double>();
    outCenterA[0] = pXX[0];
    outCenterA[1] = pXX[1];
    outCenterA[2] = pXX[2];

    return true;
}
